﻿using System;
using System.Collections.Generic;
using System.Linq;
using GPWiki;

namespace Project1
{
	/// <summary>
	/// An implementation of the A* search algorithm.
	/// </summary>
	/// <remarks>In its current state, the implementation assumes that the heuristics function is consistent.</remarks>
	class AStarSearch : AbstractSearch
	{
		/// <summary>
		/// Gets or sets the heuristic function.
		/// </summary>
		/// <value>
		/// The heuristic function.
		/// </value>
		public IHeuristic Heuristic { set; get; }

		/// <summary>
		/// Performs a search.
		/// </summary>
		/// <param name="problem">The search problem.</param>
		/// <returns>
		/// The goal state if found, otherwise <c>null</c>.
		/// </returns>
		protected override State PerformSearch(IProblem problem)
		{
			//
			// Use a binary heap for the frontier
			//
			BinaryHeap<State> frontier = new BinaryHeap<State>();
			//
			// Use a hashset to keep track of explored states (the CLOSED list)
			//
			HashSet<Object> explored = new HashSet<object>();
			//
			// Is the initial state also the goal state?
			//
			State state = problem.InitialState;
			if (problem.IsGoal(state))
			{
				return state; // Problem? There is no problem!
			}
			//
			// Calculate the heuristic value h(n) for the initial state and place it on the frontier
			//
			state.HeuristicCost = Heuristic.EstimateCost(problem, state);
			frontier.Add(state);
			//
			// Now loop until we either find the goal state or have exhausted all possibilities
			//
			while (true)
			{
				//
				// Have we emptied the frontier?
				//
				if (frontier.Count == 0)
				{
					//
					// No solution
					//
					return null;
				}
				//
				// Pop a state/node off the frontier
				//
				state = frontier.Remove();
				//
				// Performance metric bookkepping
				//
				base.StateExpanded();
				base.SetCurrentTreeSize(ExpandedStates + frontier.Count);
				//
				// Have we found the goal state?
				//
				if (problem.IsGoal(state))
				{
					return state;
				}
				//
				// Assuming that the heuristic function is consistent then we can put this state on the
				// explored (CLOSED) list since a better path to it will not be found.
				//
				explored.Add(state.Key);
				//
				// Iterate through the successor states of the current state
				//
				foreach (State successor in problem.GetSuccessors(state))
				{
					//
					// If this successor has been explored, move on to the next one
					//
					if (explored.Contains(successor.Key))
					{
						continue;
					}
					//
					// Calculate h(n) for this successor state
					//
					successor.HeuristicCost = Heuristic.EstimateCost(problem, successor);
					//
					// Is this state already on the frontier?
					//
					State oldState = frontier.FirstOrDefault<State>(x => x.Key.Equals(successor.Key)); //TODO: Store the state in a hash list too, faster lookup.
					if (oldState != null)
					{
						//
						// Yes, remove the old one if it has a higher combined cost (f(n) = g(n) + h(n))
						// Otherwise just discard this successor state and move on to the next one
						//
						if (successor.CombinedCost < oldState.CombinedCost)
						{
							frontier.Remove(oldState);
						}
						else
						{
							continue;
						}
					}
					//
					// Add this state to the frontier
					//
					frontier.Add(successor);
				}
			}
		}
	}
}
